#!/usr/bin/env python
"""
path_processor.py

读取 log.csv，处理点云轨迹生成折线路径，并保存到 path.csv；
同时生成3D可视化图 path_procc.png

处理流程：
1. 滑动窗口最小二乘线性拟合（window=20），计算点到拟合直线的偏差
2. 筛除偏差过大（离群）点，使用剩余点重拟合轨迹为分段折线
3. 合并方向相近的相邻线段（unit dir dot >=0.95）
4. 提取折线顶点保存 path.csv
5. 3D 绘制筛选后的点与折线，保存 path_procc.png
"""
import os
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

# 参数
WINDOW_SIZE = 30
OUTLIER_THRESHOLD_FACTOR = 2.0  # 离群阈值因子
MERGE_DOT_THRESHOLD = 0.98
LOG_FILE = '../maps/trajectory1.csv'
PATH_FILE = '../maps/path.csv'
FIG_FILE = '../maps/path_procc.png'

def load_points(log_path):
    df = pd.read_csv(log_path)
    pts = df[['x', 'y', 'z']].values
    return pts

def sliding_line_deviation(points, window=WINDOW_SIZE):
    N = len(points)
    deviations = np.zeros(N)
    for i in range(N - window + 1):
        window_pts = points[i:i+window]
        mean = window_pts.mean(axis=0)
        _, _, Vt = np.linalg.svd(window_pts - mean)
        direction = Vt[0]
        rel = window_pts - mean
        proj = rel.dot(direction)
        perp = rel - np.outer(proj, direction)
        dists = np.linalg.norm(perp, axis=1)
        deviations[i:i+window] = np.maximum(deviations[i:i+window], dists)
    return deviations

def remove_outliers(points, deviations, factor=OUTLIER_THRESHOLD_FACTOR):
    threshold = np.mean(deviations) * factor
    mask = deviations <= threshold
    return points[mask], threshold

def fit_polyline_by_error(points, max_dev_threshold):
    vertices = [points[0]]
    start_idx = 0
    N = len(points)
    i = start_idx + WINDOW_SIZE
    while i <= N:
        seg = points[start_idx:i]
        mean = seg.mean(axis=0)
        _, _, Vt = np.linalg.svd(seg - mean)
        dir_unit = Vt[0]
        rel = seg - mean
        perp = rel - np.outer(rel.dot(dir_unit), dir_unit)
        max_dev = np.max(np.linalg.norm(perp, axis=1))

        if max_dev > max_dev_threshold:
            vertices.append(points[i-1])
            start_idx = i - 1
            i = start_idx + WINDOW_SIZE
        else:
            i += 1

    if vertices[-1] is not points[-1]:
        vertices.append(points[-1])
    return np.array(vertices)

def merge_segments(vertices, dot_thresh=MERGE_DOT_THRESHOLD):
    if len(vertices) < 3:
        return vertices
    merged = [vertices[0]]
    dirs = [vertices[i] - vertices[i-1] for i in range(1, len(vertices))]
    dirs = [d / np.linalg.norm(d) for d in dirs]

    curr_dir = dirs[0]
    for i in range(1, len(dirs)):
        if np.dot(curr_dir, dirs[i]) >= dot_thresh:
            curr_dir = dirs[i]
            continue
        else:
            merged.append(vertices[i])
            curr_dir = dirs[i]
    merged.append(vertices[-1])
    return np.array(merged)

def save_path(vertices, path_file):
    df = pd.DataFrame(vertices, columns=['x', 'y', 'z'])
    df.to_csv(path_file, index=False, header=True)

def plot_3d(points, vertices, fig_file):
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(points[:,0], points[:,1], points[:,2], s=1, label='filtered')
    ax.plot(vertices[:,0], vertices[:,1], vertices[:,2], '-r', label='polyline')
    ax.legend()
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    plt.savefig(fig_file)
    plt.show()

def main():
    base = os.path.dirname(os.path.abspath(__file__))
    log_path = os.path.join(base, LOG_FILE)
    pts = load_points(log_path)

    deviations = sliding_line_deviation(pts)
    pts_filt, threshold = remove_outliers(pts, deviations)

    vertices = fit_polyline_by_error(pts_filt, threshold)
    vertices = merge_segments(vertices)

    save_path(vertices, os.path.join(base, PATH_FILE))
    plot_3d(pts_filt, vertices, os.path.join(base, FIG_FILE))

if __name__ == '__main__':
    main()